/*
 * CurveAction.cpp
 *
 *  Created on: 17/lug/2014
 *      Author: va2013
 */

#include "CurveAction.h"

CurveAction::CurveAction(ArRobot* r, double turningTime, int rotVel, int rotDecel,
		bool* mustCurve) :
		ArAction("CurveAction") {
	robot = r;
	this->turningTime = turningTime;
	this->mustCurve = mustCurve;
	this->startTime = cv::getTickCount();
	this->rotationVel = rotVel;
	this->achievedCurve = false;
	this->rotDecel = rotDecel;

}

CurveAction::~CurveAction() {
}

ArActionDesired* CurveAction::fire(ArActionDesired currentDesired) {

	myDesired.setRotDecel(rotDecel);
	if (*mustCurve) {

		std::cout << "curve " << (double) (direction) * rotationVel
				<< std::endl;
		myDesired.setRotVel((double) (direction) * rotationVel, 0.5);

		double elapsed = (cv::getTickCount() - startTime) / cv::getTickFrequency();

		if (elapsed >= turningTime) {
			std::cout << "stop curve" << std::endl;
			myDesired.setRotVel(0);
			*mustCurve = false;
			achievedCurve = true;
		}

	} else
		myDesired.setRotVel(0);

	return &myDesired;

}

void CurveAction::setDeltaHeading(double turningTime, int direction) {

	//stop();
	this->turningTime = turningTime;
	this->startTime = cv::getTickCount();
	achievedCurve = false;
	this->direction = direction;

}
bool CurveAction::haveAchievedCurve() {
	return achievedCurve;
}

